﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Emgu;
using Emgu.CV;
using System.Drawing;
using System.Diagnostics;
using System.Xml;
using Emgu.CV.Util;
using Emgu.CV.Structure;

namespace ContourSplit.Scaffold
{
	public sealed class Calibrator : IDisposable
	{
		private Size boardSize, imageSize;
		private Mat cameraMatrix, distCoeffs;
		private bool _calibrated;
		public bool isCalibrated { get { return _calibrated; } }
		private List<string> imageList;
		public Calibrator(int w, int h, string imgListPath)
		{
			boardSize = new Size(w, h);
			XmlDocument doc = new XmlDocument();
			doc.Load(imgListPath);
			XmlNode n = doc.SelectSingleNode("opencv_storage/images");
			imageList = n.InnerText.Split(new char[] { '\n', '\r' }).Where(x => x.Trim().Length > 0).ToList();
		}
		public void Undistort(IInputArray input, IOutputArray output)
		{
			Trace.Assert(_calibrated);
			//Rectangle roi=new Rectangle();
			//Mat newcamMat = CvInvoke.GetOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 0.7, imageSize, ref roi);
			CvInvoke.Undistort(input, output, cameraMatrix, distCoeffs/*, newcamMat*/);
			//Mat temp = new Mat(output.GetInputArray().GetMat(), roi);
		}
		public bool Calibrate(string cfgFile)
		{
			FileStorage fs = new FileStorage(cfgFile, FileStorage.Mode.Read);
			if (fs.IsOpened)
			{
				cameraMatrix = new Mat();
				distCoeffs = new Mat();
				imageSize = new Size(fs.GetNode("imageWidth").ReadInt(), fs.GetNode("imageHeight").ReadInt());
				boardSize = new Size(fs.GetNode("boardWidth").ReadInt(boardSize.Width), fs.GetNode("boardHeight").ReadInt(boardSize.Height));
				fs.GetNode("cameraMatrix").ReadMat(cameraMatrix);
				fs.GetNode("distCoeffs").ReadMat(distCoeffs);
				_calibrated = true;
			}
			else
			{
				Mat view, viewGrey = new Mat();
				VectorOfPointF pointbuf = new VectorOfPointF();
				VectorOfVectorOfPointF imagePoints = new VectorOfVectorOfPointF();
				CvInvoke.NamedWindow("Chessboard");
				for (int i = 0; i < imageList.Count; i++)
				{
					view = CvInvoke.Imread(imageList[i]);
					if (i == 0)
						imageSize = view.Size;
					else
						Trace.Assert(imageSize == view.Size);

					CvInvoke.CvtColor(view, viewGrey, Emgu.CV.CvEnum.ColorConversion.Bgr2Gray);
					bool found = false;
					try
					{
						found = CvInvoke.FindChessboardCorners(viewGrey, boardSize, pointbuf);
					}
					catch (Exception e)
					{
						Console.WriteLine(e.Message);
						Console.WriteLine(e.StackTrace);
						return false;
					}
					if (found)
						CvInvoke.CornerSubPix(viewGrey, pointbuf, new Size(11, 11), new Size(-1, -1), new Emgu.CV.Structure.MCvTermCriteria(30, 0.1));
					CvInvoke.DrawChessboardCorners(view, boardSize, pointbuf, found);
					string msg = $"{i} / {imageList.Count}";
					CvInvoke.PutText(view, msg, new Point(100, 100), Emgu.CV.CvEnum.FontFace.HersheyPlain, 1.0, new Emgu.CV.Structure.MCvScalar(255, 0, 0), 2);
					imagePoints.Push(pointbuf);
					Mat resized = new Mat();
					CvInvoke.Resize(view, resized, new Size(0, 0), 0.7, 0.7);
					CvInvoke.Imshow("Chessboard", resized);
					CvInvoke.WaitKey(1000);
				}
				cameraMatrix = new Mat(3, 3, Emgu.CV.CvEnum.DepthType.Cv64F, 1);
				distCoeffs = new Mat(8, 1, Emgu.CV.CvEnum.DepthType.Cv64F, 1);

				int k = 0;
				MCvPoint3D32f[] a = new MCvPoint3D32f[boardSize.Height * boardSize.Width];
				for (int i = 0; i < boardSize.Height; i++)
					for (int j = 0; j < boardSize.Width; j++)
						a[k++] = new MCvPoint3D32f(j, i, 0);

				VectorOfVectorOfPoint3D32F objectPoints = new VectorOfVectorOfPoint3D32F();
				for (int i = 0; i < imagePoints.Size; i++)
					objectPoints.Push(new VectorOfPoint3D32F(a));

				VectorOfMat rvects = new VectorOfMat(), tvects = new VectorOfMat();
				double rms = CvInvoke.CalibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvects, tvects, Emgu.CV.CvEnum.CalibType.Default, new MCvTermCriteria());
				Point tmp = new Point();
				_calibrated = CvInvoke.CheckRange(cameraMatrix, true, ref tmp, double.MinValue, double.MaxValue) && CvInvoke.CheckRange(distCoeffs, true, ref tmp, double.MinValue, double.MaxValue);
				if (_calibrated)
				{
					fs = new FileStorage(cfgFile, FileStorage.Mode.Write);
					fs.Write(cameraMatrix, "cameraMatrix");
					fs.Write(distCoeffs, "distCoeffs");
					fs.Write(imageSize.Width, "imageWidth");
					fs.Write(imageSize.Height, "imageHeight");
					fs.Write(boardSize.Width, "boardWidth");
					fs.Write(boardSize.Height, "boardHeight");
					fs.Dispose();
				}
				CvInvoke.DestroyWindow("Chessboard");
			}
			return _calibrated;
		}

		public void Dispose()
		{
			cameraMatrix.Dispose();
			distCoeffs.Dispose();
		}
	}
}
